// --------------------------------------------------------------------------------------------------------------------
// <copyright file="KinectProcessorV2Test.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Runtime
{
    using System;
    using System.Collections.Generic;
    using System.IO;
    using System.Runtime.Serialization;
    using System.Threading;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Vision;
    using Microsoft.Robotics.Vision.Cameras;
    using Microsoft.Robotics.Vision.Runtime.Cameras;
    using Microsoft.VisualStudio.TestTools.UnitTesting;
    using Test.Robotics.Runtime;

    /// <summary>
    /// Validates that message synchronization works as expected.
    /// </summary>
    [TestClass]
    public class KinectProcessorV2Test
    {
        /// <summary>
        /// Test the Kinect V2 agent processor by sending depth and IR V2 messages
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void KinectProcessorV2AgentTest()
        {
            this.RunProcessorTest(5, 1);
            this.RunProcessorTest(1, 4);
        }

        /// <summary>
        /// Generate a kinect V2 IR raw message
        /// </summary>
        /// <param name="rand">A random generator</param>
        /// <param name="width">Image Frame width</param>
        /// <param name="height">Image Frame height</param>
        /// <returns>The created message</returns>
        private static IRCameraAgentMessage<IRParameters> CreateIRMessage(Random rand, int width, int height)
        {
            ImageFrame<ushort> irFrame = new ImageFrame<ushort>(width, height);

            for (int i = 0; i < width * height; ++i)
            {
                irFrame[i] = (ushort)rand.Next(ushort.MaxValue);
            }

            IRCameraAgentMessage<IRParameters> msg = new IRCameraAgentMessage<IRParameters>(
                AgentMessage.GetCurrentTime(),
                irFrame,
                new IRParameters());
            return msg;
        }

        /// <summary>
        /// Generate a kinect V2 depth raw message
        /// </summary>
        /// <param name="rand">A random generator</param>
        /// <param name="width">Image Frame width</param>
        /// <param name="height">Image Frame height</param>
        /// <returns>The created message</returns>
        private static DepthCameraAgentMessage<DepthParameters> CreateDepthMessage(Random rand, int width, int height)
        {
            ImageFrameDepth depthFrame = new ImageFrameDepth(width, height);
            ushort maxDepth = 4500;

            for (int i = 0; i < width * height; ++i)
            {
                depthFrame[i] = (short)rand.Next(maxDepth);
            }

            DepthCameraAgentMessage<DepthParameters> msg = new DepthCameraAgentMessage<DepthParameters>(
                    AgentMessage.GetCurrentTime(),
                    depthFrame,
                    new DepthParameters());
            return msg;
        }

        /// <summary>
        /// Runs a kinect processor test
        /// </summary>
        /// <param name="averageCount">Count of frames to average</param>
        /// <param name="repeatCount">Repetition count for message send loop</param>
        private void RunProcessorTest(int averageCount, int repeatCount)
        {
            // Create a pipeline and send a single Kinect message
            AgentLocator producerLocator = new AgentLocator();
            MockKinectV2Agent kinectAgent = new MockKinectV2Agent("mockKinectV2");
            producerLocator.Register(kinectAgent);

            KinectAgentMessageProcessorV2 processor = new KinectAgentMessageProcessorV2("ProcessorV2", kinectAgent, averageCount);
            producerLocator.Register(processor);
            MessageCollectorAgent<DepthIRColorCameraMessage<DepthParameters, IRParameters, ColorParameters>> collector = new MessageCollectorAgent<DepthIRColorCameraMessage<DepthParameters, IRParameters, ColorParameters>>("collector", processor);
            IAgentHost collectorHost = producerLocator.Register(collector);
            collectorHost.Activate();
            int width = 10;
            int height = 15;
            Random rand = new Random(30);
            int pixIdx = rand.Next(width * height);
            int sumIR = 0;
            int sumDepth = 0;
            int lastIR = 0;
            int depthValue = 0;
            int prevDepthValue = 0;
            long origTime = 0;
            DepthIRColorCameraMessage<DepthParameters, IRParameters, ColorParameters> procMsg = null;
            int irReceiveCount = repeatCount;
            int depthReceiveCount = repeatCount;
            List<int> averages = new List<int>();

            for (int repeat = 0; repeat < repeatCount; ++repeat)
            {
                sumIR = 0;
                sumDepth = 0;

                for (int cnt = 0; cnt < averageCount; ++cnt)
                {
                    IRCameraAgentMessage<IRParameters> ir = CreateIRMessage(rand, width, height);
                    lastIR = ir.ImageFrame[pixIdx];
                    sumIR += lastIR;
                    if (0 == cnt)
                    {
                        origTime = ir.OriginatingTime;
                    }

                    kinectAgent.Publish(ir);

                    DepthCameraAgentMessage<DepthParameters> depth = CreateDepthMessage(rand, width, height);
                    if (averageCount == 1)
                    {
                        prevDepthValue = depthValue;
                    }

                    depthValue = depth.DepthFrame[pixIdx];
                    sumDepth += depthValue;
                    averages.Add(sumDepth / (cnt + 1));
                    kinectAgent.Publish(depth);
                }

                // Wait for the message to appear at end of pipeline
                collector.Wait(2500);

                Assert.IsTrue(
                    collector.Results.Count == 1,
                    string.Format("Expected 1 message at end of pipeline found {0}", collector.Results.Count));
                procMsg = collector.Results.Dequeue();
            }

            Assert.IsTrue(procMsg.OriginatingTime <= origTime);
            Assert.AreEqual(procMsg.IRFrame.Width, width);
            Assert.AreEqual(procMsg.IRFrame.Height, height);
            Assert.AreEqual(procMsg.DepthFrame.Width, width);
            Assert.AreEqual(procMsg.DepthFrame.Height, height);

            int avg = sumIR / averageCount;
            int delta = (averageCount > 1) ? lastIR / (averageCount - 1) : 0;
            Assert.AreEqual(avg, (int)procMsg.IRFrame[pixIdx], delta);

            avg = sumDepth / averageCount;

            if (averageCount > 1)
            {
                bool found = false;
                for (int i = averageCount - 1; i >= 0 && found == false; --i)
                {
                    if ((int)procMsg.DepthFrame[pixIdx] == averages[i])
                    {
                        found = true;
                    }
                }

                Assert.IsTrue(found);
            }
            else
            {
                Assert.IsTrue(depthValue == (int)procMsg.DepthFrame[pixIdx] || prevDepthValue == (int)procMsg.DepthFrame[pixIdx]);
            }

            collectorHost.Deactivate();
        }
    }
}
